#include "ParticleFilter.h"

ParticleFilter::ParticleFilter() {
	//	binX = 0.5f, binY = 0.5f, binTheta = 15.0f * PI/180.0f ;
	HighestEverConfidence = -INF;
	stepsTaken = 0;
}

ParticleFilter::~ParticleFilter() {
}

void Filter::init() {
	HighestEverConfidence = -INF;
	stepsTaken = 0;
}

tf::Transform Filter::publishBestTransform(DDD &BestPose, double confidenceInBestPose,
		const tf::Transform &currentTransform, double overallUncertainity) {
	/**
	 * The best method to pick the best transform is to publish the best transform that we have now
	 * We tried 1) Publishing the mean transform of the biggest bin given by the KLD
	 * 2) Publishing the mean transform of the points around the mean of the biggest bin of the KLD
	 * 3) Publishing the mean transform of the points around the best transfom we have now
	 */
	if (HighestEverConfidence < confidenceInBestPose) {
		HighestEverConfidence = confidenceInBestPose;
		MyPose p;
		p.setPoseInMap(BestPose.first.first, BestPose.first.second, BestPose.second, listener);
		return p.getMapOdomTransform();
	} else {
		/* 1) When in arbitrary positions, the bestConfidence is very low, and tends to vary wildly within a small range
		 * In such cases, it is best not to make a guess on a correct transform and stick to what we have
		 * 2) It is also best not to disturb the current transform when the currently published transform has a better
		 * confidence
		 **/
		return currentTransform;
	}
}

tf::Transform Filter::publishBestTransform(MyPose &pose, double confidenceInBestPose,
		const tf::Transform &currentTransform, double overallUncertainity) {
	/**
	 * The best method to pick the best transform is to publish the best transform that we have now
	 * We tried 1) Publishing the mean transform of the biggest bin given by the KLD
	 * 2) Publishing the mean transform of the points around the mean of the biggest bin of the KLD
	 * 3) Publishing the mean transform of the points around the best transfom we have now
	 */
	if (HighestEverConfidence < confidenceInBestPose) {
		HighestEverConfidence = confidenceInBestPose;
		return pose.getMapOdomTransform();
	} else {
		/* 1) When in arbitrary positions, the bestConfidence is very low, and tends to vary wildly within a small range
		 * In such cases, it is best not to make a guess on a correct transform and stick to what we have
		 * 2) It is also best not to disturb the current transform when the currently published transform has a better
		 * confidence
		 **/
		return currentTransform;
	}
}

tf::Transform ParticleFilter::reSample(PoseArray &pa, WeightsForPoses &w, MotionModel &mm,
		SensorModel &sm, MATRIX &map, Robot &robot, OdomMove &om,
		const tf::Transform &currentTransform) {
	if (stepsTaken > 5)
		Filter::init();
	stepsTaken++;
	//ROS_INFO("resampling");
	PoseArray newPoses;
	DDD bestPose;
	double x, y, theta;
	double confidenceInBestPose = -INF;
	mm.stepRobot(pa, om, robot, newPoses, map);
	sm.estimateWeights(newPoses, robot, w);
	//ROS_INFO("Roulette %d",newPoses.size());
	pa.clear();
	std::vector<int> pos;
	RouletteWheelSample(w, pos);
	for (int i = 0; i < newPoses.size(); i++) {
		int item = pos[i];
		while (!validParticle(newPoses[item], map))
			item = pos[rand()%newPoses.size()];
		if (confidenceInBestPose < w[item]) {
			confidenceInBestPose = w[item];
			newPoses[item].getPoseInMap(x, y, theta);
			bestPose = DDD(DD(x, y), theta);
		}

		pa.push_back(newPoses[item]);
	}
	//ROS_INFO("resampled");
	return publishBestTransform(bestPose, confidenceInBestPose, currentTransform, pa.size());
}
